#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <ceres/ceres.h>

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

using namespace std;
using namespace cv;

void findmatch(const Mat &img1, const Mat &img2, vector<KeyPoint> &keypoints_1, 
               vector<KeyPoint> &keypoints_2, vector<DMatch> &goodmatches)
{
    Mat descriptors1, descriptors2;
    Ptr<FeatureDetector> detector=ORB::create();
    Ptr<DescriptorExtractor> descriptor=ORB::create();
    Ptr<DescriptorMatcher> matcher =DescriptorMatcher::create("BruteForce-Hamming");
    
    detector->detect(img1, keypoints_1);
    detector->detect(img2, keypoints_2);
    
    descriptor->compute(img1, keypoints_1, descriptors1);
    descriptor->compute(img2, keypoints_2, descriptors2);
    
    vector<DMatch> matches;
    matcher->match(descriptors1, descriptors2, matches);
    
    auto min_max=minmax_element(matches.begin(),matches.end(),[](DMatch &m1, const DMatch &m2)
    {
        return m1.distance<m2.distance;
    });
    double min_dist=min_max.first->distance;
    double max_dist=min_max.second->distance;
    
    for(int i=0; i<matches.size();i++){
        if(matches[i].distance<max(2*min_dist,30.0)){
            goodmatches.push_back(matches[i]);
        }
    }
};

void getdateset(const vector<DMatch> &goodmatches, const vector<KeyPoint> &keypoint1, 
                const  vector<KeyPoint> &keypoint2, double *x1, double *y1, double *x2, 
                double *y2)
{
    for(int i=0; i<goodmatches.size();i++){
        *(x1+i)=keypoint1[goodmatches[i].queryIdx].pt.x;
        *(y1+i)=keypoint1[goodmatches[i].queryIdx].pt.y;
        *(x2+i)=keypoint2[goodmatches[i].queryIdx].pt.x;
        *(y2+i)=keypoint2[goodmatches[i].queryIdx].pt.y;
    }      
};

struct essentionResidual{
    essentionResidual(double x1, double y1, double x2, double y2):x1_(x1),y1_(y1),x2_(x2),y2_(y2){}
    template<typename T>
    bool operator() (const T *const m, T *residual) const {
        T a1=*(m+0)*x1_+*(m+1)*y1_+*(m+2);
        T a2=*(m+3)*x1_+*(m+4)*y1_+*(m+5);
        T a3=*(m+6)*x1_+*(m+7)*y1_+*(m+8);
        residual[0]=x2_*a1+y2_*a2+a3;
        return true;
    }
private:
    const double x1_, y1_, x2_, y2_;
}; 

void dataopt(const double *x1, const double *y1,
             const double *x2, const double *y2,
             const int &numkeycouple, const int & numite, double *m){
   
    ceres::Problem problem;
    
    for(int i=0; i<numkeycouple; ++i){
    problem.AddResidualBlock(
        new AutoDiffCostFunction<essentionResidual,1,9>(
            new essentionResidual(*(x1+i),*(y1+i),*(x2+i),*(y2+i))
                                                  ),
                             NULL,
                             m
            );
    }
    
    Solver::Options options;
    options.max_num_iterations=numite;
    options.linear_solver_type=ceres::DENSE_QR;
    options.minimizer_progress_to_stdout=true;
    
    Solver::Summary summary;
    ceres::Solve(options, &problem,&summary);
    std::cout<<summary.BriefReport()<<"\n";
    std::cout<<*(m+0)<<" "<<*(m+1)<<" "<<*(m+2)<<"\n";
    std::cout<<*(m+3)<<" "<<*(m+4)<<" "<<*(m+5)<<"\n";
    std::cout<<*(m+6)<<" "<<*(m+7)<<" "<<*(m+8)<<"\n";
};

void gettrans(const Mat &K, Mat &KT)
{
    for(int i=0;i<K.rows;i++){
        for(int j=0; j<K.cols; j++){
            KT.at<double>(j,i)=K.at<double>(i,j);
        }
    }
};


void getK(const double &fx,const double &cx, const double &fy, const double &cy, Mat &K, Mat &invKT)
{
    K=(Mat_<double>(3,3)<<fx,0,cx,0,fy,cy,0,0,1);
    Mat KT=Mat(K.rows,K.cols,CV_64F);
    gettrans(K,KT);
    cv::invert(KT,invKT,cv::DECOMP_LU);
};

Mat pmut(const Mat &K, const Mat &T)
{
    cv::Mat R=cv::Mat(K.rows,K.cols,CV_64F);
    for(int i=0;i<K.rows; i++){
        for(int j=0;j<K.cols; j++){
            double value=0;
            for(int k=0; k<K.cols; k++){
                value+=K.at<double>(i,k)*T.at<double>(k,j);
            }
            R.at<double>(i,j)=value;
        }
    }
    return R;
}
    

Mat getEssen(const Mat &m,  const double &fx, const double &fy, const double &cx,
                 const double &cy)
{
    Mat essen;
    Mat K, INVKT;
    getK(fx,cx,fy,cy,K,INVKT);
    essen=pmut(INVKT,pmut(m,K));
    return essen;
}


int main(int argc, char **argv) 
{
    
    Mat image1=imread("../1.jpg", IMREAD_ANYCOLOR);
    Mat image2=imread("../2.jpg", IMREAD_ANYCOLOR);
    assert(image1.data!=nullptr&&image2.data!=nullptr);
    
    vector<KeyPoint> keypoint1, keypoint2;
    vector<DMatch> goodmatches;
    findmatch(image1, image2, keypoint1, keypoint2, goodmatches);
    cout<<goodmatches.size()<<endl;
     
    double x1[goodmatches.size()],y1[goodmatches.size()],x2[goodmatches.size()],y2[goodmatches.size()];
    getdateset(goodmatches,keypoint1,keypoint2,x1,y1,x2,y2);
    
    
    Mat findEssentialMat( InputArray points1, InputArray points2,
                                 InputArray cameraMatrix, int method = RANSAC,
                                 double prob = 0.999, double threshold = 1.0,
                                 OutputArray mask = noArray() );
    
    
    double a00=1,a01=1,a02=1,a10=1,a11=1,a12=1,a20=1,a21=1,a22=1;
    double m[9]={a00,a01,a02,a10,a11,a12,a20,a21,a22};
    dataopt(x1,y1,x2,y2,100,30,m);
    Mat essention;
    double cfx=1220.0,ccx=960,cfy=1220,ccy=540;
    Mat mm=(Mat_<double>(3,3,m));
    essention=getEssen(mm,cfx,cfy,ccx,ccy);
    return 0;
}














